/*
    Copyright 2010, 2011 Eric Wong
	contact@optimalcycling.com
	http://www.optimalcycling.com
	Optimal Cycling - Advanced power pacing program for cyclists
	
	This file is part of Optimal Cycling.

    Optimal Cycling is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Optimal Cycling is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with Optimal Cycling.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __OPENCL_VERSION__
	#include "Integrator.h"
#endif

inline int RKF45CheckValid(real4* iData, 
					  real4* iParams1, 
					  real2* iParams2,
					  __global const real4* iConstants, 
					  int result, real S)
{
	if(!isfinite((*iData).s3) || result == 0)
    {
		(*iData).s0 = (*iData).s1 = (*iData).s2 = (*iData).s3 = 0;

        (*iParams2).s1 = 1;

        // If H > (*iConstants).s1, then we decrease H
        if ((*iParams2).s0 > (*iConstants).s1)
        {
            if (S <= THREE_QUARTERS)
                (*iParams2).s0 = S * (*iParams2).s0;
            else							
				(*iParams2).s0 = real_divide((*iParams2).s0, TWO);			

			(*iParams2).s0 = clamp((*iParams2).s0, (*iConstants).s1, (*iConstants).s2);

            return -2;
        }
        else
            // But if H <= (*iConstants).s1, then we return -1
            return -1;
    }

	// Okay
	return 0;
}

inline int RKF45Step(
		real4* iData,
		real4* iCorrectionData,
		real4* iParams1,
		real2* iParams2,
		__global const real4* iConstants,
		real* iPower,
		__global const real* powerVal,
		__global const real4* coursePosPoints, 
		__global const real* coursePosLinear,
		const real coursePosLinearMax,
		__global const real4* coursePosDirection,
		__global const real4* coursePosInfo,
		__global const real4* riderProp,
		__global const real4* wind, 
		__global const real4* climate,
		const sizeT numSimPoints,

		__global const real* dragCoeffWheelYawAngles,
		__global const real2* dragCoeffWheels, 
		const sizeT dragCoeffWheelsNumPts,

		__global const ModelInfo* modelInfo)
{
	real8 k, K, M;
	int result;
	real Err, S;

#ifdef KAHAN_SUMMATION
	real corrected_next_term, new_sum;
#endif


    M.s0 = (*iParams1).s2;
    K.s0 = (*iParams2).s0 * M.s0;
    k.s0 = (*iParams1).s3;
    M.s1 = (*iParams1).s2 + ONE_QUARTER * (*iParams2).s0 * k.s0; 
    K.s1 = (*iParams2).s0 * M.s1;  
	(*iData).s1 = (*iParams1).s1 + ONE_QUARTER * K.s1;
	(*iData).s2 = M.s1;
    result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
	result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, 1);
	if (result != 0)
		return result;
	k.s1 = (*iData).s3;

    M.s2 = (*iParams1).s2 + THREE_EIGHTH * (*iParams2).s0 * k.s1;
    K.s2 = (*iParams2).s0 * M.s2;
	(*iData).s1 = (*iParams1).s1 + THREE_THIRTY_SECOND * K.s0 + NINE_THIRTY_SECOND * K.s1;
	(*iData).s2 = M.s2;
    result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
	result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, 1);
	if (result != 0)
		return result;
	k.s2 = (*iData).s3;
    
    M.s3 = (*iParams1).s2 + FRAC_12_13 * (*iParams2).s0 * k.s2;
    K.s3 = (*iParams2).s0 * M.s3;
	(*iData).s1 = (*iParams1).s1 + FRAC_1932_2197 * K.s0 - FRAC_7200_2197 * K.s1 
		+ FRAC_7296_2197 * K.s2;
	(*iData).s2 = M.s3;
    result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
	result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, 1);
	if (result != 0)
		return result;
	k.s3 = (*iData).s3;

    M.s4 = (*iParams1).s2 + (*iParams2).s0 * k.s3;
    K.s4 = (*iParams2).s0 * M.s4;
	(*iData).s1 = (*iParams1).s1 + FRAC_439_216 * K.s0 - EIGHT * K.s1 + 
		FRAC_3680_513 * K.s2 - FRAC_845_4104 * K.s3;
	(*iData).s2 = M.s4;
    result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
	result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, 1);
	if (result != 0)
		return result;
	k.s4 = (*iData).s3;
    
    M.s5 = (*iParams1).s2 + ONE_HALF * (*iParams2).s0 * k.s4;
	(*iData).s1 = (*iParams1).s1 - FRAC_8_27 * K.s0 + TWO * K.s1 - FRAC_3544_2565 * K.s2 
		+ FRAC_1859_4104 * K.s3 - FRAC_11_40 * K.s4;
	(*iData).s2 = M.s5;
    result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
	result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, 1);
	if (result != 0)
		return result;
	k.s5 = (*iData).s3;

    // Difference between the fifth order and fourth order estimate    
    Err = fabs((*iParams2).s0 * (FRAC_1_360 * k.s0 - FRAC_128_4275 * k.s2 - 
		FRAC_2197_75240 * k.s3 + FRAC_1_50 * k.s4 + FRAC_2_55 * k.s5));

	if(!isfinite(Err))
    {		
        (*iParams2).s1 = ONE;
        if ((*iParams2).s0 <= (*iConstants).s1)
		{			
            return -1;
		}
        else
        {
            (*iParams2).s0 = ONE_TENTH * (*iParams2).s0;

            if ((*iParams2).s0 < (*iConstants).s1)
                (*iParams2).s0 = (*iConstants).s1;

            return -2;
        }
    }
    else if (Err < SMALL_REAL)
        S = FOUR;
    else
    {		
		S = powr(real_divide((*iConstants).s0 * (*iParams2).s0, TWO * Err), ONE_QUARTER);   // x^y where x is always positive, required for powr     

		if (!isfinite(S))
            // This means that Err is very close to zero since (*iConstants).s0 and H are known to be valid
            // This means we need to increase H
            S = FOUR;
		else
			S = clamp(S, ONE_TENTH, FOUR);
    }

    if ((Err <= (*iConstants).s0) || ((*iParams2).s0 <= (*iConstants).s1) || (S >= ONE))
    {
#ifdef KAHAN_SUMMATION
		(*iData).s0 = (*iParams1).s0;
		corrected_next_term = (*iParams2).s0 - (*iCorrectionData).s0;
		new_sum = (*iData).s0 + corrected_next_term;
		(*iCorrectionData).s0 = (new_sum - (*iData).s0) - corrected_next_term;
		(*iData).s0 = new_sum;

		(*iData).s1 = (*iParams1).s1;
		corrected_next_term = (FRAC_25_216 * K.s0 + FRAC_1408_2565 * K.s2 
			+ FRAC_2197_4104 * K.s3 - FRAC_1_5 * K.s4) - (*iCorrectionData).s1;
		new_sum = (*iData).s1 + corrected_next_term;
		(*iCorrectionData).s1 = (new_sum - (*iData).s1) - corrected_next_term;
		(*iData).s1 = new_sum;

		(*iData).s2 = (*iParams1).s2;
		corrected_next_term = ((*iParams2).s0 * (FRAC_25_216 * k.s0 + FRAC_1408_2565 * k.s2 
			+ FRAC_2197_4104 * k.s3 - FRAC_1_5 * k.s4)) - (*iCorrectionData).s2;
		new_sum = (*iData).s2 + corrected_next_term;
		(*iCorrectionData).s2 = (new_sum - (*iData).s2) - corrected_next_term;
		(*iData).s2 = new_sum;
#else
        (*iData).s0 = (*iParams1).s0 + (*iParams2).s0;

        (*iData).s1 = (*iParams1).s1 + FRAC_25_216 * K.s0 + FRAC_1408_2565 * K.s2 
			+ FRAC_2197_4104 * K.s3 - FRAC_1_5 * K.s4;
        (*iData).s2 = (*iParams1).s2 + (*iParams2).s0 * (FRAC_25_216 * k.s0 + FRAC_1408_2565 * k.s2 
			+ FRAC_2197_4104 * k.s3 - FRAC_1_5 * k.s4);
#endif

        result = ModelAcceleration(iData, INTEGRATOR_VARS_TO_MODEL);
		result = RKF45CheckValid(iData, iParams1, iParams2, iConstants, result, S);
		if (result != 0)
			return result;

        (*iParams1).s0 = (*iData).s0;
        (*iParams1).s1 = (*iData).s1;
        (*iParams1).s2 = (*iData).s2;
        (*iParams1).s3 = (*iData).s3;

        if ((*iParams2).s1 > 0)
        {
            if (S < ONE)
                (*iParams2).s0 = S * (*iParams2).s0;
        }
        else
            (*iParams2).s0 = S * (*iParams2).s0;

		(*iParams2).s1 = S < ONE ? ONE : NEG_ONE;
		(*iParams2).s0 = clamp((*iParams2).s0, (*iConstants).s1, (*iConstants).s2);

        return 0;
    }
    else
    {
        if ((*iParams2).s1 > 0)
        {
            if (S < ONE)
                (*iParams2).s0 = S * (*iParams2).s0;
        }
        else
            (*iParams2).s0 = S * (*iParams2).s0;

		(*iParams2).s1 = S < ONE ? ONE : NEG_ONE;
		(*iParams2).s0 = clamp((*iParams2).s0, (*iConstants).s1, (*iConstants).s2);

        return -2;
    }
}



